from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # Get package directory
    package_dir = get_package_share_directory('fruit_detector')
    model_path_a = os.path.join(package_dir, 'model', 'A.onnx')
    model_path_b = os.path.join(package_dir, 'model', 'B.onnx')
    model_path_c = os.path.join(package_dir, 'model', 'C.onnx')
    
    # Launch arguments
    left_image_topic = LaunchConfiguration('left_image_topic', default='/left_cam/image_raw')
    right_image_topic = LaunchConfiguration('right_image_topic', default='/right_cam/image_raw')
    conf_threshold = LaunchConfiguration('conf_threshold', default='0.5')
    iou_threshold = LaunchConfiguration('iou_threshold', default='0.45')
    
    # Declare launch arguments
    declare_left_image_topic = DeclareLaunchArgument(
        'left_image_topic',
        default_value='/left_cam/image_raw',
        description='Topic name for the left camera image'
    )
    
    declare_right_image_topic = DeclareLaunchArgument(
        'right_image_topic',
        default_value='/right_cam/image_raw',
        description='Topic name for the right camera image'
    )
    
    declare_conf_threshold = DeclareLaunchArgument(
        'conf_threshold',
        default_value='0.5',
        description='Confidence threshold for object detection'
    )
    
    declare_iou_threshold = DeclareLaunchArgument(
        'iou_threshold',
        default_value='0.45',
        description='IoU threshold for non-maximum suppression'
    )
    
    # Create node
    fruit_detector_node = Node(
        package='fruit_detector',
        executable='fruit_detector_node',
        name='fruit_detector',
        parameters=[{
            'left_image_topic': left_image_topic,
            'right_image_topic': right_image_topic,
            'model_path_a': model_path_a,
            'model_path_b': model_path_b,
            'model_path_c': model_path_c,
            'conf_threshold': conf_threshold,
            'iou_threshold': iou_threshold
        }],
        output='screen'
    )
    
    return LaunchDescription([
        declare_left_image_topic,
        declare_right_image_topic,
        declare_conf_threshold,
        declare_iou_threshold,
        fruit_detector_node
    ]) 